#include "problem.h"
#include <iostream>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <cmath>
#include <vector>
using namespace Eigen;
using namespace ceres;

double f = 115.0;
double x_0 = 0;
double y_0 = 0;

Vector3d intersection_similarity_transform(const DataIntersection &data) {
    // 1.计算基线初值
    Eigen::Vector3d B;
    B = data.X_eo[1] - data.X_eo[0];

    // 2.计算像点坐标的像空间辅助坐标系坐标
    Vector3d p;
    p << data.p_img[0].x(), data.p_img[0].y(), -f;
    Vector3d X1 = data.R_eo[0] * p;
    p << data.p_img[1].x(), data.p_img[1].y(), -f;
    Vector3d X2 = data.R_eo[1] * p;
    // 3.计算点投影系数
    double N1, N2;
    N1 = (B.x() * X2.z() - B.z() * X2.x()) / (X1.x() * X2.z() - X2.x() * X1.z());
    N2 = (B.x() * X1.z() - B.z() * X1.x()) / (X1.x() * X2.z() - X2.x() * X1.z());
    // using namespace std;
    // cout << data.X_eo[0] <<endl<< data.p_img[0]<<endl;
    // cout << N1 << "|" << N2 << endl;
    // 4.计算地面坐标
    Eigen::Vector3d XA;
    XA.x() = data.X_eo[0].x() + N1 * X1.x();
    XA.y() = (data.X_eo[0].y() + N1 * X1.y() + data.X_eo[1].y() + N1 * X2.y()) / 2;
    XA.z() = data.X_eo[0].z() + N1 * X1.z();

    return XA;
}

struct ProjectionResidual {
    ProjectionResidual(const Matrix3d &R, const Vector3d Xs, Vector2d point2d) : Xs(Xs), point2d(point2d), R(R) {}

    template <typename T> bool operator()(const T *XA, T *residual) const {

        T X = XA[0] - Xs[0];
        T Y = XA[1] - Xs[1];
        T Z = XA[2] - Xs[2];

        T x = -f * (R(0, 0) * X + R(1, 0) * Y + R(2, 0) * Z) / (R(0, 2) * X + R(1, 2) * Y + R(2, 2) * Z) + x_0;
        T y = -f * (R(0, 1) * X + R(1, 1) * Y + R(2, 1) * Z) / (R(0, 2) * X + R(1, 2) * Y + R(2, 2) * Z) + y_0;

        residual[0] = point2d.x() - x;
        residual[1] = point2d.y() - y;
        return true;
    }

private:
    Vector2d point2d;
    Matrix3d R;
    Vector3d Xs;
};

Vector3d intersection_least_squares(const DataIntersection &data, const Vector3d &X_initial) {
    // 已知值x0,y0,f,xs,ys,zs,phi,omga,kappa
    // 观测值x1,y1,x2,y2
    // 未知数xA,yA,zA
    Vector3d Xi = X_initial;
    // ceres 优化求解
    ceres::Problem problem;
    std::vector<ceres::ResidualBlockId> blocks;

    {
        ceres::CostFunction *cost = new ceres::AutoDiffCostFunction<ProjectionResidual, 2, 3>(
            new ProjectionResidual(rodrigues(data.r_eo[0]), data.X_eo[0], data.p_img[0]));
        problem.AddResidualBlock(cost, nullptr, Xi.data());
    }
    {
        ceres::CostFunction *cost = new ceres::AutoDiffCostFunction<ProjectionResidual, 2, 3>(
            new ProjectionResidual(rodrigues(data.r_eo[1]), data.X_eo[1], data.p_img[1]));
        problem.AddResidualBlock(cost, nullptr, Xi.data());
    }
    ceres::Solver::Options op;
    op.max_num_iterations = 100;
    op.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    ceres::Solve(op, &problem, &summary);
    // std::cout << summary.BriefReport() << "\n";
    // std::cout << "inital:" << X_initial << std::endl << "final:" << Xi << std::endl;
    return Xi;
}